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Abstract. In this paper we will develop and design numerical methods for 
optimal control problems related to a class of underactuated Lagrangian me- 
chanical systems where the configuration manifold is a trivial principal bundle. 
We will construct these geometric integrators using discrete variational calculus, 
that is deriving a discrete version of the higher-order Euler-Lagrange equations 
on trivial principal bundles. The analysis applies to systems subject to higher- 
order constraints (that is, depending on higher-order derivatives as, for example, 
the acceleration). Interesting applications as, for instance, a discrete derivation 
of the Euler-Lagrange equations for higher-order Lagrangians and higher-order 
reduced Lagrangians, respectively, are shown. We find interesting applications 
both in the optimal control of an underactuated vehicle and the well-known plate 
ball problem, the latter seen as an optimization problem with nonholonomic con- 
straints. 

I. Introduction 

In the last years, the study of Lagrangian reduction and reconstruction for me- 
chanical systems with Lie group symmetries has drawn an extraordinary interest 
in different areas of science as engineering, economy, physics, etc. The goal of this 
paper is to study, from a geometric point of view, variational integrators for opti- 
mal control problems of mechanical systems defined on a trivial principal bundle 
and its applications in optimal control theory. Our motivation is the control of 
the class of underactuated mechanical systems. This type of mechanical systems, 
i.e. the underactuated, is characterized by the fact that there are more degrees of 
freedom than actuators. 

The presence of underactuated mechanical systems is ubiquitous in engineering 
applications as a result, for instance, of design choices motivated by the search of 
less cost devices or as a result of a failure regime in fully actuated mechanical sys- 
tems. The underactuated systems include spacecraft, underwater vehicles, mobile 
robots, helicopters, wheeled vehicles, mobile robots, underactuated manipulators, 
etc. 

We extend the theory of discrete mechanics based on discrete variational calculus 
for systems which depend on higher-order derivatives and subject to constraints 
(also depending of higher-order derivatives). We use Hamilton's principle and La- 
grange multiplier theorem on manifolds in order to obtain discrete paths that ap- 
proximately satisfy the dynamics and the constraints. This is achieved by formulat- 
ing a higher-order discrete variational problem subject to higher-order constraints. 
Such formulation gives us the preservation of important geometric properties of 
the mechanical system, such as momentum, symplecticity, group structure, good 
behavior of the energy, etc [23J. 
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A typical optimal control problem on a trivial principal bundle consists on find- 
ing a trajectory of the state variables and controls (q(t), g(t),u(t)) given fixed 
initial and final conditions (g(0),g(0)) and (q(T), g(T)) respectively, and, as well, 
minimizing the cost functional defined by 



here, q(t) belongs to the configuration space Q, g(t) evolves on a Lie group G and 
u{t) on the space of admissible controls, usually a subspace of R m . In the previous 
definition of the cost functional we are considering the final time T as a variable 
in the optimization problem. Nevertheless, we shall usually consider T fixed. 

Our approach is based on recently developed structure-preserving numeric in- 
tegrators for optimal control problems (see [T71 HH E31 EU [351 US] and references 
therein) based on solving a discrete optimal control problem as a discrete higher- 
order variational problem with higher-order constraints (see [H [TBI ELT] for the 
continuous case) which are used for simulating and controlling the dynamics for 
satellites, spacecrafts, underwater vehicles, mobile robots, helicopters, wheeled ve- 
hicles, etc [9]. 

Finally, the case when the system is subject to nonholonomic constraints is 
also of great interest in optimal control of mechanical systems. Systems which 
nonholonomic constrains are carefully studied in jH [51 [91 [29]. Some geometric 
integrators for this type of systems are developed in [TBI EH1 ED 1251 S3]- Thus, we 
also consider how our framework on discrete variational integrators with constraints 
is a suitable framework for optimal control of discrete nonholonomic mechanical 
systems. The class of systems that we study in this paper includes wheeled vehicles, 
such as robots on wheels and/or tracks. As an example, we pay attention to the 
optimal control of an homogeneous ball rotating in a plate. 

1.1. Organization of the paper. The paper is structured as follows. In §|2] we re- 
call some results about variational integrators, Hamilton's principle on Lie groups 
and the discrete Euler-Poincare equations. In §([3]) we derive the continuous second- 
order Euler-Lagrange equations for trivial principal bundles from Hamilton's prin- 
ciple. Moreover, we study the higher-order case. The proposed method appears in 
^4] where we construct from a discretization of the Lagrangian and through discrete 
variational calculus the discrete higher-order Euler-Lagrange equations on trivial 
principal bundles. These equations are derived using a discrete Hamilton's princi- 
ple. In the last section, we study continuous and discrete higher-order mechanics 
for systems subject to higher-order constraints. We apply these techniques in §([5]), 
to the optimal control of underactuated mechanical systems, e.g. the optimal con- 
trol of systems defined on the Lie group SE(2), and finally, we study the optimal 
control of the plate ball problem. 

2. Discrete Mechanics and Variational Integrators 

Let Q be a n-dimensional different iable manifold, the configuration manifold, 
with local coordinates (g l ), 1 < % < n. Denote by TQ its tangent bundle with 
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induced coordinates (q l , q 1 ). Given a Lagrangian function L : TQ — > R, the Euler- 
Lagrange equations are 

d fdL\ dL 

T ( tttt J - 7^ = 0, 1< I < n. 1 
dt \dq* J dq l V ' 

These equations are a system of n implicit second order differential equations. 
In the sequel, we will assume that the Lagrangian is regular, that is, the matrix 

( dtfdqi ) * s non " srn g u l ar - From Lagrange's theorem, it is well known that the origin 

of these equations is variational (see [Ij HT])- 

The variational integrators [12] are derived from a discrete variational principle. 
These integrators also retain some of main geometric properties of the continuous 
system, such as simplecticity, momentum conservation and a good behavior of the 
energy associated with the Lagrangian system (see [23] and references therein). In 
the sequel we will review the construction of this type of geometric integrators. 

A discrete Lagrangian is a map L^: Q x Q — > R, which may be considered 
as an approximation of the integral action defined by a continuous Lagrangian 
L: TQ R, 



Ld(qo,qi) ~ / L(q(t),q{t)) dt, 
Jo 



where q(t) is a solution of the Euler-Lagrange equations for L; g(0) = go, q(h) = q\ 
and the time step h > is small enough. 

Define the action sum Ad '■ Q N+1 — > R, corresponding to the Lagrangian Ld by 



N 



Ad — Ld(qk-i,qk), 



k=l 

where qt € Q for < k < N, where N is the number of steps. The discrete 
variational principle then requires that 5Ad = where the variations are taken 
with respect to each point qk, 1- < k < N — 1 along the path, and the resulting 
equations of motion; (a system of difference equations) given fixed endpoints q 
and q^, are 

D 1 L d (q k , qk+i) + D 2 L d (q k _ 1 , q k ) = 0, (2) 

where D\ and D 2 denote the derivative to the Lagrangian respect the first and 
second arguments, respectively. 

These equations are usually called discrete Euler-Lagrange equations. Under 
some regularity hypotheses (the matrix (Di 2 Ld(qk, <?fc+i)) is regular), it is possible 
to define a (local) discrete flow T Ld : QxQ QxQ, by Y Ld (q k _i, q k ) = (q k , q k+l ) 
from g. 

We introduce now the two discrete Legendre transformations associated to Ld- 
F-Ld-.QxQ -> T*Q 

(Qo,Qi) *-> (qo,-DxL d (q ,qi)) , 

(3) 

F + L d :QxQ -> T*Q 

(Qo,Qi) | -> (qi,D 2 L d (q ,qi)) , 
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and the discrete Poincare-Cartan 2-form u d = (F + L d )* ojq = (F~L d )* uq, where 
ujq is the canonical symplectic form on T*Q. u d is a symplectic form if the discrete 
Lagrangian is regular, which is indeed equivalent to F~L d (or F + L d ) being a local 
diffeomorphism [12] . 

The discrete algorithm determined by T^ d preserves the (pre-) symplectic form 
on T*(Q x Q), u d , i.e., Y* Ld u d = u d . Moreover, if the discrete Lagrangian is 
invariant under the diagonal action of a Lie group G, then the discrete momentum 
map J d : Q x Q -> q* defined by 

(Jd(qk,qk+i),0 = (D 2 L d (q k ,q k+1 ),£ Q (q k+1 )) 

is preserved by the discrete flow. Therefore, these integrators are symplectic- 
momentum preserving. Here, £q denotes the fundamental vector field determined 
by £ G 0, where is the Lie algebra of G, 

for q G Q (see [12] for more details). 

2.1. Discrete Mechanics on Lie groups: Euler-Poincare equations. In this 
section we recall the basics on discrete mechanics on Lie groups and Hamilton's 
principle on Lie groups for the formulation of the Euler-Poincare equations. 

If the configuration space is a Lie group G, then the discrete trajectory is rep- 
resented numerically using a set of N + 1 points (go, gi, . . . , g^) with $ G G, 
< i < N. 

A way to discretize a continuous problem is using a retraction map t : q — > G 
which is an analytic local diffeomorphism which maps a neighborhood of G to 
a neighborhood of the neutral element e G G. As a consequence, it is possible to 
deduce that t(£)t(— £) = e for all £ G $j. The retraction map is used to express 
small discrete changes in the group configuration through unique Lie algebra ele- 
ments (see [31]), namely = r~ 1 (g k ~ 1 g k+1 )/h, where £ 0- That is, if £ k were 
regarded as an average velocity between g k and g k +i, then r is an approximation to 
the integral flow of the dynamics. The difference g^ 1 g k+ \ G G, which is an element 
of a nonlinear space, can now be represented by the vector in order to enable 
unconstrained optimization in the linear space for optimal control purposes. 

It will be useful in the sequel, mainly in the derivation of the discrete equations 
of motion, to define the right trivialized tangent retraction map as a function 
dr : x g -»■ q by 

1Y(£) • 5 = Tr T ($dT{($), 
where 5 G and r : G x G — > G is the usual right translation on the group. Here 
we use the following notation, dr^ := dr(£) : — > Q. The function dr is linear in 
its second argument. From this definition the following identities hold (see [7] for 
further details): 

Proposition 2.1. Given a map r : g — > G, its right trivialized tangent dr^ : — > Q 
and its inverse dr^ 1 : — > 0, are such that for g = r(£) G G and 77 G 0, the 
following holds 

3^(0*7 = dr e 7 7 r (0 and d ^\9)v = drr^r/r (-£)). 
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The most natural example of retraction map is the exponential map at the 
identity e of the group G, exp e : g — > G. We recall that, for a finite-dimensional 
Lie group, exp e is locally a diffeomorphism and gives rise a natural chart |40j . 
Then, there exists a neighborhood U of e G G such that exp" 1 : U — > exp ( T 1 (f/) is 
a local C°° diffeomorphism. A chart at g G G is given by = exp" 1 o £ g -i, where 
£ denotes the usual left-translation of an element of the group £ : G x G — >■ G. 

In general, it is not easy to work with the exponential. For instance, if we 
are considering matrix groups, the right trivialized derivative and its inverse are 
defined by infinite series 



where Bj are the Bernoulli numbers, x, y G g and ad^ y = [x, y] is the usual matrix 
bracket (see |23j). Tipically, these expressions are truncated in order to achieve a 
desired order of accuracy. 

In consequence it will be useful to use a different retraction map. More con- 
cretely, the Cayley map (see [7J [23] and the Appendix for further details) will 
provide us a proper framework in the examples shown below. 

The following theorem, regardless of the retraction structure locally relating G 
and 0, gives us the relation between the discrete Euler-Lagrange equations and the 
discrete Euler-Poincare equations. 

Theorem 2.1. [39J Let G be a Lie group and Ld : G x G — > K be a discrete 
Lagrangian function. We suppose that Ld is left-invariant over the diagonal action; 
that is, L d (gg k , 99k+x) = Ld(g k , 9k+i) with g G G. Let Ld : G —> ffi. be the restriction 
to the identity (that is, L d : (G x G)/G ~ G -> R, L d {g k 1 g k+1 ) = L d (g k , g k+1 )). 
For a pair of points (gk,g k +i) G G x G, we consider W k = g k yk+x- Then the 
following assertions are equivalent: 

1) (^fc)fcLo extremize the discrete action J2 k =Q L d (g k , g k +x) f or a ^ variation with 
initial and final fixed points. 

2) The discrete Euler-Poincare equations r^ L' d {W k )—£* w _ L' d (Wk-i) = 0, k — 
1, . . . , N hold, where £ and r are the left- and right-translation of the Lie group and 
' denote the partial derivative. 

3) ( W k)k=o extremize Ylk=o L d(W k ) for all variations 5W k = -rj k W k + W k r] k+1 
with t)q = T]n = 0; where r] k G Q is given by r\ k = g k Sg^. 

The previous assertions are also equivalent to say that (g k ) k=0 satisfy the Euler- 
Lagrange equations for Ld. 

3. Higher-order Euler-Lagrange equations on trivial principal 



3.1. Higher-order tangent bundles. In this subsection we recall some basic 
facts of the higher-order tangent bundle theory. At some point, we will particularize 
this construction to the case when the configuration space is a Lie group G. For 
more details see [TUl EH] ■ 
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Let Q be a differentiable manifold of dimension n. It is possible to introduce an 
equivalence relation in the set C fc (R, Q) of /c-differentiable curves from R to Q. By 
definition, two given curves in Q, 71 (i) and 72(i), where t G (—a, a) with a G R 
have contact of order k at q = 71 (0) = 72(0) if there is a local chart (</?, U) of Q 
such that qo G U and 



d s d s 

(V°7i(*)) =^l(v ? °72(t)) 



t=o 



for all s = 0, k. This is a well-defined equivalence relation in C fc (R, Q) and the 
equivalence class of a curve 7 will be denoted by [7]^ • The set of equivalence classes 
will be denoted by T^Q and it is not hard to show that it has a natural structure 

of differentiable manifold. Moreover, Tq : T^Q — > Q where Tq ([y]^^ = 7(0) is 

a fiber bundle called the tangent bundle of order k of Q. 

When there exists a group action <fi : G x Q — > Q, we shall naturally define a lift 
to the group action (fc) : G x T (fe) Q ->■ T^Q given by 

^(Mf) :=[^o 7 ]f). 

This action endows T^Q with a principal G-bundle structure. The quotient 
T^Q/G is a fiber bundle over the base <5/C The class of elements [7]^ in the 
quotient (T^Q/G) is denoted [[7]^ . 

Now, let G be a Lie group and consider, as before, the left-translation on itself 

I: GxG^G, (g,h) ^ £ g (h) = gh . 

Obviously l g is a diffeomorphism (the same is valid for the right-translation, but 
in the sequel we only work with the left-translation, for sake of simplicity). 

The left-translation allows us to trivialize the tangent bundle TG and the cotan- 
gent bundle T*G as follows 

TG Gxq, (g,g)^(g,g- 1 g) = (g,T g l g -ig) = (g,0, 
T*G -+ Gxq*, (g,a g )^(g,T:e g (a g )) = (g,a) , 

where q = T e G is the Lie algebra of G and e is the neutral element of G. In the 
same way, we have the following identifications: TTG = G x 3$j (where 3$j stands 
for 0X0X0. Throughout this paper, the notation n V, where V is a given space, 
denotes the cartesian product of n copies of V) and T*TG = G x q* x q x q*. 
Therefore, in the case when the manifold Q has a Lie group structure, i.e. Q = G, 
we can also use the left trivialization to identify the higher-order tangent bundle 
T^G with G x kg. That is, if g : I ->• G is a curve in CW(R, G): 

T (fc) . T (fc) G — ^ G x kg 



t=0 



It is clear that T^ fc ^ is a diffeomorphism. 

We will denote = g~ l and this equation shall be considered in the 
following as the continuous reconstruction equation. Therefore 
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where 

^\t) = ^ l (g-\t)g(t)), 0<l<k-l 

and g(0) = g,^ l \0) = £™,0 < I < k — 1. We will use the following notations 
without distinction = £, £W = £, when referring to the derivatives. 

We may also define the surjective mappings Tq : T^G — > T®G, for I < k, 

given by Tq^ ([g]^^ = \g]$ • With the previous identifications we have that 

4' k \g(o), e(o), e(o), . . . , £ (fc - 1} (o)) = fo(o), £(o), £(o), . . . , ^~ 1} (o)) 

It is easy to see that T«G eGxj, T^G = G and 4°' fc) = r£. 

Now, we consider the canonical immersion : T^G — > T(T^ k ~^G) defined as 
j k ([g} (k) ) = [g^ 1 ^, where g^-V is the lift of the curve g to T^G] that is, the 

curve g( k ~V : R -> T^'^G is given by # {/c_1) (£) = where = + s )- 

Using the identification given by T^^ we have that: 

: C7 x % — ► G x (2k - 1)q 

fate c A - l: i (^,^,e,---,e {fe - 2) ; 

where we identify T(T^G) = T(G x (k - l)g) = G x (2k - l)g, in the natural 

way 



3.2. Euler-Lagrange equations for trivial principal bundles. Now, we de- 
rive from a variational procedure the Euler-Lagrange equations for the trivial prin- 
cipal bundle Q = M x G where M is a n— dimensional differentiable manifold with 
coordinates (q l ), 1 < i < n, and G a Lie group. 

Let L : TQ — > R be a Lagrangian function. Since TQ ~ TM x TG and 
TG ~ G x q from the left-trivialization, we consider our Lagrangian function as 
L : TM x G x g -> R. 

The motion of the mechanical system is described by applying the following 
principle, 



5 f L(q(t),q(t),g(t),t(t))dt = (4) 
Jo 

for all variations 5q(t) where 5q(0) = Sq(T) = 0, q(t) £ M and 5£ verifying 
5£(t) = Tj(t) + [£(t), r/(t)], where is an arbitrary curve on the Lie algebra with 
77(0) = Tj(T) = and 77 = g~ 1 5g (see [21] )• This principle gives rise to the Euler- 
Lagrange equations on trivial principal bundles given by 

d (dL\ _dL 

dt\dq~J-W { ] 

*U)=^U) + '»v (5b) 
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where ad^r/ = [£,rj\. If the Lagrangian L is left-invariant the above equations are 
written as 



d_ /dL 
dt \dq 
d f5L 



dL 
dq 



(6a) 
(6b) 



which shall be considered as the Euler-Poincare equations for Lagrangian systems 
defined on trivial principal bundles. 

3.3. Second-order Euler-Lagrange equations for trivial principal bundles. 

In this subsection we deduce, from a variational principle, the Euler-Lagrange 
equations for Lagrangians defined on T^Q ~ T^MxGx2q from a left-trivization. 

Let L : T^Q ~ T (2) MxGx2$j -> R be a Lagrangian function, L(q,q,q, g, g,g) = 
L(q, q, q, g, £, £) where £ = g~ l g. The problem consists on finding the critical curves 
of the action defined by 



A(c(t)) 



L(q,q,q,g,£,£)dt 



among all the curves c(t) G Q°°(T^M x G x 2$j) satisfying the boundary conditions 
for arbitrary variations 5c = (5q, Sq^\ 5q( 2 \ 5g, 5£, <5£), where Sq = ^| e=0 ? e , Sq^ = 
4rSq, for I — 1,2; and <5g = 4-\ £=0 g e . Here, e4 j e and e i— )■ g e are smooth curves 
in G and M respectively, for e G (—a, a) C R, such that go = g and go = We 
define, for any e, £ e := g~ x 'g e - The corresponding variations 5£ induced by <5g are 
given by = rj + [£, rj] where r\ := g _1 Sg G Q (5g = grj). Therefore 



5A(c(t)) = 5 / L(q(t),q(t),q(t),g(t),{;(t),i(t))dt 
Jo 

d <- T 



de 



, L(q £ (t), q e (t), Ut),9e(t),Ut),Ut))dt 

1 / .dL . . .dL d .. ,9L d 2 ,, .dL . . 
(^T, Sq) + ( — , -(<Jg)) + ( — , -,(Sq)} + (-=-, 5g) 



dq 



dq ' dt 
,5L d 



dq ' dt 2 



dg 



Employing the integration by parts (twice) and the vanishing initial and endpoint 
conditions q(0) = q{T) = q(0) = q{T) = and r](0) = rj(T) = t)(0) = rj(T) = 0, 
the stationary condition 5A = implies 



d A (5L d 5L\ \, f T 



dt 



t rT / d f d dL dL\ dL 



o \dt \dt dq dq 



dq 



5q)dt = 0. 



;(g'l.i>* 
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Therefore, SA(c(t)) = if and only if c(t) G Q°°(T^M x G x 2g) is a solution of 
the Euler-Laerange equations for L : T^M x G x 2g ->• R, 

dt ad vl^ dtTU~ 9 W [ ) 

d (dL _ d dL\ _ dL 

dt \dq~ ~ didq 7 ) ~ 'dq' ( ' 



which splits into a M part (7a) and a G part (7b). From equations (|7j) we can to 



obtain the following theorem 

Theorem 3.1. Let L : T^Q ~ T^M x G x 2g ^ R fe a left-trivialized second- 
order Lagrangian, £ = g -1 g, and n(t) a curve on q with fixed endpoints 7/(0) = 
T){T) = 0. r/ie cnrwe c(t) G 6°° (T^M x G x 2g) satisfies 5A(c(t)) = /or tae 
actzon 71 : 6°° (T^M x G x 2g) R azven 6y 

■4( c (*)) = / L{q,Q,<i,g,t,£)dt, 

Jo 

with respect to the variations 5q, 6q® = ^Sq for I = 1,2 (such that 5q(0) = 
Sq(T) = and 5q(0) = Sq(T) = 0); 5g and 5£ = rj + ad^n, if and only if c(t) is a 
solution of the Euler- Lagrange equations, 



dL ± _d_bL_ d^SL A *$L_ ,* f± SL 

^~7tJz + G¥^ + ad ^~ t\dtl^ 



d 2 dL d dL dL () 
dt 2 dq dt dq dq 



which are just the expansion of . 



+ ad*— -ad*(-— ) =0, (8a) 



Corollary 3.1. If the Lagrangian is left-invariant, that is if L does not depend of 
g G G, the equations of motion are given by 

d^6L_d L 6L A JJ^_ A * ( ?L 5 J± 
dT 2 Jl~JtJi + ^ ~ « {dt 5£ 

d 2 dL _ ddL dL _ 

dt 2 dq dt dq dq 

which shall be considered the Euler- Poincare equations for second order trivial 
principal bundles. 

Remark 3.2. Higher-order Euler-Lagrange equations on trivial principal 
bundles. The previous setting can be extended to Lagrangians defined on a higher- 
order tangent bundle of a configuration manifold given by a trivial principal bundle. 
We identify the higher-order tangent bundle T^Q = T^(M x G) ~ T^M xGx 
kg. 

Let L be a higher-order Lagrangian defined on T^ k 'M x G x k$ where we have 
local coordinates (q, q, q, q^ k \ g, ^ k ~ 1 '), £ = g~ l g. Let us denote the 
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variations 

for / = 1, . . . , k; j = 1, . . . , k — 1 and where the variation 5£ is induced by Sg as 
<5£ = ?7+ [£, ^] where 77 is a curve on the Lie algebra with fixed endpoints. Therefore, 
we can deduce that from Hamilton's principle, integrating k times by parts and 
using the boundary conditions 



Sq(0) = 5q(T) = 0, 

5qd\0) = 8qV\T) = 0, j = l,...,k 
v (0) =77(0) = ... =r]^- 1 \0) =0, 
v (T)=r ] (T) = ...=r ] ^- 1 \T) = 0, 

(and therefore, ££^(0) = 6£®(T) = 0, for / = l,...,k) the higher-order Euler- 
Lagrange equations for the Lagrangian L : T^M x G x k$ — > R are 




As in the previous cases, if the Lagrangian is left-invariant the right hand side of 
the second equation vanishes. 



4. Discrete higher-order Euler-Lagrange equations on trivial 

principal bundles 

In this section we derive, from a discrete variational point of view, the discrete 
Euler-Lagrange equations for Lagrangians defined on a higher-order tangent bundle 
to Q = M x G, that is T^M x G x kg, where G is a finite dimensional Lie group 
and its Lie algebra. This variational procedure gives rise to obtain a variational 
integrator. As an application, we will consider the optimal control of mechanical 
systems. 

4.1. Discrete second-order Euler Lagrange equations on trivial principal 
bundles. Let G7 be a finite dimensional Lie group and consider the associated 
discrete problem. We chose 3(M x 67) = 3M x 3G as the natural discretization of 
since we are considering the left trivialization T^G ~ G x 2$j. Therefore, 
we develop the discrete Euler-Lagrange equations for a discrete Lagrangian La : 
3(M x G) — > R. We define the reconstruction equation Wu = g^dk+i- Taking 
variations and considering = g k l 5gk we obtain 

5W k = -Y, k W k + WfcEfc+i, (9) 



where g k , W k G G and S fe G 0. 
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Definition 4.1 (Discrete Hamilton's principle for trivial principal bun- 
dles). Let define the space of sequences Q 2 ( N+1 ) = {(q , q x , . . . , qN, 9o,9i,---, 9n) £ 
(N+l)Mx(N+l)G}, assuming that (q ,go), (qi,9i), (qN-i,9N-i) and (<?Ar,£?iv) are 
fixed, we define the discrete action associated with the Lagrangian L d : 3(Mx G) — > 
R as 

N-2 

■A d (qo:N, go-.N-2, W -.n-i) = Y L d (q k , q k+1 , q k+2 , g k , Wfc, Wk+l). (10) 

k=0 

Hamilton's principle establishes that the sequence (q , ?i, . . . , qN, go, gi, ■ ■ ■ , 9n) G 
q2(n+i) £ S a so \ u ii on j ffo e Lagrangian system determined by L d : 3(M xG)->R 
if and only if (q , q u . . . , q N , g , g u . . . , g N ) is a critical point of A d . 

The equations of motion are the critical paths of the discrete action, then, 5A d = 
if and only if 

N-2 

= L d (q k , qk+i, qk+2, gk,Wk, W k+ i) (11) 

k=0 

N-2 / 3 6 

= Y ^2( D 3 L d)^Qj+k-i + (D 4 L d )5g k + J2( D 3 L d)8W j+k - 

k=0 \j=l j=5 

Here, D\ denotes the partial derivative with respect to the I— th variable. Rear- 
ranging the sum indexes, (11) can be decomposed in the following way: 

JV-2 3 

Y J2( D J L d) 5 qj+k-i = 

k=0 j=l 
N-2 

Y( D i L d(qk, q k+1 ,q k+2 ,g k , W k , W k+X ) + D 2 L d (q k -i, q k , q k +i, g k -i, W k -x, W k ) 

k=0 

+D 3 L d (q k _ 2 , qk-i, qk, 9k-2, W k -2, Wk-i))5q k , 



N-2 N-2 6 

Y^(D 4 L d )5g k + Y ^2(DjL d )SW j+k - 5 = 

k=0 k=0 j=5 

N-2 N-2 3 

Y DiL d (g k , W k , W k +i)5g k + YY1 D i L d{9k, W k , W k+1 )5W j+k _ 2 = 

k=0 k=0 j=2 

N-2 

Y D iLd{9k, W k , Wk+i) (gk^k) 

k=0 

N-2 3 

+ Y Y D J L d(9k, W k , W k +l) (-Ej- +fc _ 2 W^ +fc _2 + W j+k -2^j+k-l) , 
k=0 j=2 

where we have used the relations S fc = g^ l 8g k and Moreover, from the second 
row we have considered that the M variables (q k , q k +i, qk+2) are fixed. From these 
equalities we obtain the following theorem: 
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Theorem 4.1. Let L : T (2) Q R be a Lagrangian where Q = M x G and T^Q 

is left-trivialized as T (2) Q ~ T (2) M x G x 2g. Let us consider c £ e 2(Ar+1) . T/jen, 
toe patfi c safe/ies = for A d {c(t)) : e 2 ^ +1 ) -»■ R, given in definition (|4.1l) 



TOt/t respect to arbitrary variations 5qk,Sgk,SWk, also satisfying (q , E ), 

(qN-i, Sjv_i), (giv,S7v) /ixe<i ; = g^Sgk, if and only if c(t) satisfies the discrete 

Euler- Lagrange equations 

= DiL d \ a (q k , qk+i, qk+2) + ^2Az| g (9fc-i> 9fc; Qk+i) + -^3-^1 (9fc-25 9ft+i; 9fc); 

- r^D 3 L d | g (^ fc _i, W fc _i, W k ) + e* Wk i D 3 L d \ q (g k „ 2 , W k - 2 , W k -i), 
for k = 2,..., N- 2. 

The notation L d \ implies that the M variables are frozen while, correspondingly, 
Ld\ g implies that the G variables are frozen. 

Corollary 4.2. If L d is left-invariant then we c an d efine the reduced Lagrangian 



L d : 3M x 2G — > R and the equations in theorem J^A 



are rewritten as 



— DiLd\ g {qk, 9ft+i) Qk+2) + D 2 Ld\ g (qk-i, qk, 9ft+i) + D 3 Ld\ g (qk-2, 9ft+i; 9ft); 
- r^ fc D 2 L d | g (W fc _i, W k ) + t Wk i D 2 L d \ q (Wk-2, W fe _i), 

/or = 2,...,N — 2. These should be consider as the discrete Euler- Poincare 
equations for second order trivial principal bundles. 

Remark 4.3 (Discrete higher-order Euler-Lagrange equations). Is easy to 
extend these techniques for higher order discrete mechanics. Consider a mechanical 
system determined by a Lagrangian L : T^(M x G) — > R. It is well known 
that the tangent bundle T^ k \M x G) can be left-trivialized as T^(M x G) ~ 
T^M x G x kg, where g is the Lie algebra G. 

Now, we consider the associated discrete problem by replacing the higher order 
tangent bundle by (k + 1) copies of the manifold and the group. At this point, we 
develop the discrete Euler-Lagrange equations for the discrete Lagrangians defined 
on (k + 1)(M x G) = (k + 1)M x (k + 1)G. 

Let Ld '■ {k + 1)(M x G) — > K be a discrete Lagrangian where G is a finite 
dimensional Lie group and M a n-dimensional differentiate manifold. As before, 
denote by Wi = g^gi+i and Ej = g^ 1 5g i . Taking variations over W{ we obtain 
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SWi = —T^iWi + WiE i+ i, where g^Wi 6 G and e q. By Hamilton's principle 
the equations of motion are the critical paths of the discrete action 

N-k 

,i+k-X)J 

with boundary conditions g (0 ,fc-i) = q( N -k+x,N) = 0, £(o,k-i) = 0, S (iV _ fc+ i iAr) = 
and g , . . . , g k _i and gN-k+x, ■ ■ ■ ,gN fixed, where we employ the notation 

= Qi+l> ■ ■ ■ , Qj-x, Qj), 
W (iJ) = (W l ,W l+1 ,...,W J . 1 ,W j ), 

Taking variations we deduce that, 

N-k 
i=0 

N-k k+X 

^2 [ D x L d\ q (9ii W (i,i+k)) (9&i) + ^2D j L d \ g (q {i _ j+lii _ j+1+k) )6q i 

i=k j=l 
k+X 

+ DjL d \ q (gi, W / ( iii+fc _i)) (-Sj + i-2^ + i_2 + W / i+i _ 2 S i+ i_i) , 
i=2 

where again we have used the relation Q. Therefore, the discrete higher-order 
Euler- Lagrange equations on (k + 1)(M x G) are given by 

fc+i 

= ^ Z?jL d | g (g^_ 3 - +1|i _j + i +fc )), 
j=i 

= t^^L^i-uW^i+k-!)) 

k+X 

+ ^2 (fwi-i) D j L d\ q (gi-j+x,W(i- j+hi - j+k) ) 

3=1 
k+X 

~ fe) D 3 L d\ q (9i-j+2, W(i- j+2 ,i-j+k+X)), 

J'=2 

where k < i < N — k. These equations, together with the reconstruction equation 
Wj = g^ gi + \ are called discrete higher-order Euler- Lagrange equations on (A; + 
1)(M xG). 

Finally, if L d is left-invariant we will define the reduced Lagrangian L d : (k + 
1)M x A;G — >■ R. Then the discrete higher-order Euler- Poincare equations on the 
reduced space (k + 1)M x kG are given by 
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fc+1 

= ^ DjL d \^(q^ j+lti _ j+1+k )), 
fc+i 

Yl (fwi-i) D jLd\ q (W {i - j+1 ,i- j+k) ) 



fc+i 

= 

J'=2 
fc+1 

~~ 5^ ( r ^) £> J'- £ 'd| 9 ( Wr (i-J+2,i-J+fc+l))> 

W 7 * = ft _1 ^+i, 
for k < i < N - k. 

5. Discrete and continuous mechanical systems with constraints 
on higher-order trivial principal bundles 

In this section we derive, from a discrete variational principle, an integrator for 
higher-order mechanics with higher-order constraints. As an application, we em- 
ploy the presented techniques to solve optimal control problems for underactuated 
systems. 

5.1. Mechanical systems defined on higher-order trivial principal bundles 
subject to constraints. Consider a higher-order Lagrangian systems given by 
L : T^M x G x kg — >■ R with higher-order constraints given by <£> a : T^M x 
G x kg — > R, 1 < a < m. We denote by M the constraint submanifold locally 
determined by the vanishing of these m constraints. Define the action sum 

A(c(t)) = [ L(c(t))dt, 
Jo 

where c(t) is a curve in T^M x G x kg with local coordinates 

c(t) = (q(t), q(t), . . . , q^(t),g(t),at),i(t), • • • , C^)- (12) 

The pure variational principle for this kind of higher-order mechanical systems is 
given by 

min A(c(t)) 

subject to $ a (c(t)) = 0, 1 < a < m 

Moreover, we shall consider the boundary conditions g(0) = q(T) = g^(0) = 
g (0(T) = Ofor I = l,...,jfe; 77^(0) =rfi)(T) = for j = 0, . . . , k- 1; and f = cT 1 */. 
Here, r/(t) is a curve in the Lie algebra g with fixed endpoints induced by the 
variations <5£ = t) + [£,77]. 

Definition 5.1. A curve c (t) E e°°(T (fc) M x G x kg) will be called a solution of 
the higher-order variational problem with constraints if c is a critical point of A\^. 

As in the case of systems defined in a tangent bundle TM, and subject to 
constraints on the same tangent bundle, by using the Lagrange multipliers theorem 
we may characterize the regular critical points of the higher-order problem with 
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constraints as an unconstrained problem for an extended Lagrangian system. (See 
[32J for a detailed proof.) 

Proposition 5.1 (Variational problem with higher-order constraints). A 

curve c t e°°(T(*)M x G x kg) is a critical point of the variational problem with 
higher-order constraints if and only if c is a critical point of the functional 

[ L(c(t),\(t))dt, 
Jo 

where A = (Ai, . . . , A m ) are regarded as generalized coordinates on R' m . Moreover, 
L : T^M x G x kg x R m -> R is defined by 

Z(c(t),A) = L(c(t))-A Q $ Q (c(t)). 

The equations of motion given by the higher-order variational principle with 
constraints read: 

h dtl \ d 1 il) &? (0 /' 

fd A .,d l ( dL , d$ a \ ( dL , d<5> a \ 

= $°(c(t)), 
= 

for 1 < a < m. These are called the Euler-Lagrange equations with higher-order 
constraints on T^Q x R m , where Q = M x G. 

If the Lagrangian is left-invariant (that is, L does not depend of the variables 
on G) these equations are rewritten as the higher-order Euler-Lagrange equations 
with higher-order constraints on T^M x k$ x R m , 

A , d l f dL <9$ Q \ 

= $ a (c(t)), 
5 = 

for 1 < a < m. These shall be considered the Euler-Poicare equations with 
higher-order constraints on T^Q x R m , where Q = M x G. 

5.2. Discrete variational problem with constraints on higher-order trivial 
principal bundles. In this subsection we consider the discrete version of higher- 
order Lagrangian systems subject to higher-order constraints. These elements are 
denoted, respectively, by L d : (k + 1)(M x G) -»■ R and $^ : (k + 1)(M xG)-yl, 
for 1 < a < m. Again, to define the discrete problem we take the Lie group G 
as the discrete version of the Lie algebra g. We denote by the constraints 
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submanifold locally determined by the vanishing of these m constraints. In the 



discrete case, the curve (12) is replaced by the sequence 

{q(i,i+k),gi,W( it i +k -i)} , (13) 

for i = 1, N — k. Let define the discrete action sum by 



N-k 

■A-d(qo:N, 90:N-k, Wo:N-l) = ^ Af(9(i,i+fc)j 9ii 

i=0 



Therefore, we can consider the following problem as higher-order discrete varia- 
tional calculus with constraints: 



min Ad 

subject to : $2 (?(<,<+*) >ft> W( i)i+fc _i)) = 0, 



where g( ,fc-i), q(N-k+i,N), 0(o,fc-i), 9(N-k+i,N) are fixed, Wi = g, 1 g i+1 and the 
indices have the following range: a = 1, m, i = 0, N — k. 

It is well-know that this classical optimization problem with higher-order con- 
straints is equivalent to the following unconstrained higher-order variational prob- 
lem for 



defined on (k + 1) (M xG)x IR m with g (M+fc) e (k+l)M,gi e G, X a = (Ai,...,A m ) G 
and Wfai+k-i) ^ kG with i = 0, N — k. Consider the discrete action sum 



i> III 



N-k 



Ad(qo.N, gO:N-k,W 0: N-l, X°' N k ) — / J £rf(g(i,i+fc) ; 9i, A„), 



i=0 



where A*- ^ fe ) = (A , A w and A* is a vector with components A^, 1 < a < m. 
The unconstrained higher-order variational problem is defined as min Ad where 
<?(o,fc-i), g(o,k-i), q(N-k+i,N), g(N-k+i,N) are fixed and % = 0, ...,N -k. The critical 
points of the unconstrained problem, will be those annihilating dAj/dqi, and the 
constraints equations dAd/dX l a . Thus, the higher-order discrete Euler-Lagrange 
equations with constraints on (k + 1)(M x G) are 
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= t g% _ x (^^(^W^i,^^ 

k+l 

+ [fwi-^j (Di L d\ q {gi-j+i,W {i _ j+lti _ j+k) ) 

3=2 
k+l 

(^) ( K D j L d\ q (9i-j+2,W^ j+2> i-j+k+l)) 

3=2 

+\^ +2 D^ a d \ q (g^ +2 , W {i - jm+k+1) )) 
k+i 

= ^ DjL>d\ g (Q(i-j+i,i-j+i+k)) +^a~ J+1 - D i^d| fl (?(i-j+i,i-i+fc+i))> 

3=1 

1 = k, N — k, 



= $d(9(i,i+fc)>fl , i> W(i,i+fc-l))> 

Wi = with 0<z<iV-fc, 



with g( ,fc-i),g(7v-fc+i,7V),5'(o,fc-i) ) fl , (Af-fc+i,7V) fixed points on M and G respectively. 



5.3. Application to optimal control of underactuated mechanical sys- 
tems. The proposal of this subsection is to study optimal control problems in the 
case of underactuated mechanical systems, that is, a Lagrangian control system 
such that the number of the control inputs is less than the dimension of the config- 
uration space ( "superarticulated mechanical system" following the nomenclature 

byi). 

We shall consider the configuration space Q as the trivial principal bundle Q = 
MxG, where, as in the previous section, G is a Lie group and M is a n— dimensional 
differentiable manifold. In what follows we assume that all the control systems are 
controllable, that is, for any two points xq and Xf in the configuration space Q, 
there exists an admissible control u(t) defined on some interval [0, T] such that the 
system with initial condition xq reaches the point Xf in time T (see jH [9] for more 
details). 

Define the control manifold U C !R r where u(t) G U is the control parameter. 
Consider the left-trivialized Lagrangian L : TQ ~ TM x g — > R, (where q is the 
Lie algebra associated to the Lie group G). The equations of motion of the system 
shall be considered the controled Euler-Lagrange equations 
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1 (If) - 1? = (14a) 

where we denote by S a = {([i a ,r] a )}, fi a (q) G T*M, ^ a (g) 65*, a = 1, . . . , r; and 
A = 1, . . . ,n. Here, we are assuming that {(/i a ,?7 a )} are independent elements of 
F(T*M x 0*) and w a are admissible controls. Taking this into account, the op- 
timal control problem can be formulated as: finding trajectories (q(t),£(t),u(t)) 
of the state variables and control inputs satisfying (14), subject to initial condi- 
tions (q(0), q(0), £(0)) and final conditions (q(T), q(T), £(T)), and, extremizing the 
functional 

d(q,q,M= [ C(q(t),q(t),£(t),u(t))dt. (15) 



We can reformulate this optimal control problem as a higher-order order vari- 
ational problem subject to higher-order constraints by the following procedure: 
complete !B a to a basis {2> a ,!B a } of the vector space T*M x q*. Take its dual 
basis {S a ,S Q } on T(TM x g) = £(M) x e°°(M,g). This basis induces coordi- 
nates (V,g A ,r,D on TM x . If we denote by T> a = {{X a , Xa )} G T(TM x q) 

9 . 



(resp. 3 a = {(X Q , Xa )} G T(TM x S )), where X a ,X a G X(M); X a = Xf(g)^ 
X a = X A {q)J^x and Xa(<z)i Xa(?) G g, g G M then equations (14) are rewritten as 



d 


( 9L ) 


9L 


dt 


\dq A ) 


dq A 


d 


(dL\ 


dL 


dt 


\dq A ) 


dq A 



x ^ + {i{^)-{^)) x ' iq)=u " (16a) 
^KI(fH^€>)*« (5)=a < 16b > 

As mentioned before, the proposed optimal control problem is equivalent to a vari- 
ational problem with second order constraints (see [I] and reference therein), where 
we define the Lagrangian given, in the selected coordinates, 

by 

L(q A ,q A ,q A ,e,e) = C (q A ,q A ,C, F a (q A , q A , q A , C , C)) , 
where C is the cost function considered in ( 15 ) and 

MM) - (I (£) - ^) + (1 (|) - (-?|)) *.(,)• 

The Lagrangian L is subjected to the second-order constraints: 

W,M) - (| (g) - + (| (|) - («*£))*.(,). 

Thus, this kind of problems fits in the setting introduced in £5.1 by considering 
k = 2 and left invariance with respect to the group G and is illustrated in the 
subsequent subsections. 
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Remark 5.2. It is possible to extend our analysis to systems with external forces 
/ given by the following diagram 

TM x g > T*M x g* 



M 



just by adding the corresponding terms in the right hand side of (14). These 
equations are therefore rewritten as 

d ( dL\ dL . 



dt \dq A J dq 

<nf)~ ad| (w; 



d fdL\ „ fdL\ a . . -. . . 



where 



/ : TM x q — > T*M x g* 

(q,q,0 1 — ► (f(q,q,£),f(q,q,€)), 

such that f(q,q,0 = fA(q,q,Qdq A and f(q,q,£) G 0*. 

5.3.1. Optimal control of an underactuated vehicle. Consider a rigid body moving 
in SE(2) with a thruster to adjust its pose. The configuration of this system is 
determined by a tuple (x, y, 9, 7), where (x, y) is the position of the center of mass, 
9 is the orientation of the blimp with respect to a fixed basis, and 7 the orientation 
of the thrust with respect to a body basis. Therefore, the configuration manifold 
is Q = SE{2) x S* 1 (see [5] and references therein), where (x,y,9) are the local 
coordinates of SE(2) and 7 is the local coordinate of S 1 . 
The Lagrangian of the system is given by its kinetic energy 

L(x, y, 9, 7, x, y, 9, 7) = l -m{x 2 + y 2 ) + - J x 9 2 + l - J 2 {9 + 7 ) 2 , 

and the input forces are 

E 1 = cos(9 + 7) dx + sm(9 + 7) dy — p sin ■yd9, 
F 2 = d 7 , 

where the control forces that we consider are applied to a point on the body with 
distance p > from the center of mass (m is the mass of the rigid body), along the 
body x-axis. Note this system is an example of underactuated mechanical system 
when the configuration space is a trivial principal bundle. 

The system is invariant under the left multiplication of the Lie group G = SE(2): 

$ : SE{2) x SE{2) xS 1 ^ SE{2) x S 1 

((a, b, a), (x, y, 9, 7)) 1 — > (x cos a — ysina + a, x sin a + ycosa + b, 9 + a, 7). 

A basis of the Lie algebra se(2) = R 3 of SE(2) is given by 




e x = 1 u u , e 2 = u u u , e 3 
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from we have that 

[ei, e 2 ] = e 3 , [d, e 3 ] = -e 2 , [e 2 , e 3 ] = 0. 
Thus, we can write down the structure constants as 

ro2 rol -i ro2 /ol -i 

^31 ~~ ^23 ~~ ^13 ~~ ^32 — 1 

and all others zero. An element £ G se(2) is of the form £ = £i ei + £ 2 e 2 + £ 3 e 3 ; 
therefore the reduced Lagrangian I : T5 1 x se(2) — >• R is given by 

1(7, 7, = + £ 2 2 ) + ^^3 2 + ^37 + 1 7 2 - 



Then the reduced Euler-Lagrange equations with controls (14) in this case are 
given by 

m£i = Mi cos 7, 
m£ 2 + (Ji + ^)£i£3 + ^£i7 - m^ 3 = u x sin 7, 
(«/i + ^2)6 + ^27 -m£ 2 (£i + £3) = -uipsin7, 

^2(6+7) = «2- 

On the other hand, choosing the adapted basis {!B a , S^} the modified equations 
of motion ( 16 ) read in this case as 



m(cos7£i + sin7(£ 2 - £i£ 3 )) + (Ji + J 2 )£i£ 3 sin7 + J 2 £i7sin7 = m, 
m(cos7(£2-£ 1 £ 3 )-sin7£ 1 )+£ 1 £ 3 (Ji + J 2 )cos7 + J 2 £i7C0S7 = 0, 

p p p 

Now, we can study the optimal control problem that consists, as mentioned before, 
on finding a trajectory of state variables and control inputs satisfying the previous 
equations from given initial and final conditions (7(0), 7(0), £(0)), (j(T), j(T), £(T)) 
respectively, and extremizing the cost functional 



/ (piu\ + p 2 u\) dt, 
Jo 



where p\ and p 2 are constants. 

The related optimal control problem is equivalent to a second-order Lagrangian 
problem with second-order constraints defined as follows (see [3] for more details). 
Extremize 

A= [ Z(£,£,7,7, 7 )^, 
Jo 

subject to second-order constraints given by 

$ x = m(cos7(£ 2 - £i£ 3 ) - sin7£i) + £i£ 3 (Ji + J 2 ) cos 7 + J 2 £i7cos7, (18a) 

$ 2 = ^^(£3 + p£i£s) + J Ai + K17) + -(£ 2 - 6£ 3 - Ml±M). (18b) 
p p p 
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Here, L : T^S 1 x 2se(2) -> R is defined by 

£(7,7,7,60 = (19) 
pi (m(cos76 + sin 7(6 - 66)) + (<A + ^2)66 sin 7 + J 2 67 sin 7) 
+P2J|(6 + 7) 2 , 

which basically is the cost function C = p\u\ + p2^2 i n terms of the new variables. 



Discrete setting: Following the prescription in theorem 4.1 and the further 



conclusion in corollary |4.2[ we shall consider a discrete Lagrangian and discrete 
constraints when approaching the discrete associated problem. Moreover, since we 
are dealing with a constrained problem, we must include the constraints in the 
variational procedure as shown in §5.2| Therefore, the discrete Lagrangian and 
constraints read: T d : 3(5 1 ) x 2SE(2) — > IR, $° : 3(5 1 ) x 2SE(2)j-+ R, a = 1, 2. 
Furthermore, as was introduced in £}2j the discrete Lagrangian Ld is chosen as a 
suitable approximation of the action sum J Q T L dt. Thus, we shall set consider 

,lk+l,lk+2,W k ,Wk+l) = 

,1 k +7fc+i +7fc+2 lk+2 ~ Ik lk+2 ~ 27fc+i + 7fc T-^Wfc) T-^Wfc+i) - ^(Wk) 
3 2h ' h 2 h ' /i 2 j 

- 7fc + 7fc+i + 7fc+2 7fc+2 ~ Ik 7fc+2 ~ 27fc+i + Ik r'^Wfe) r'^Wfc+i) - T~ l (W k ) . 
3 ' 2h ' h 2 h ' /i 2 



where r is a general retraction map as shown in £ 2.l[ L is defined in ( Jl9| and $ Q 
are defined in (18). Here, lk,lk+i,lk+2 £ 5 11 while Wk,W k+ i G SE(2) (note that 



we are taking a symmetric approximation to 7&, that is 7fc+7fc + 1+7fc+2 ). In addition, 
we are taking the usual discretizations for the first and second derivatives, that is 



Ik 
7fc 

6 



lk+2 - Ik 

2h ' 
7fc+2 - 27fc +i + ik 
h 2 

6+1 — 6 
h ' 



where se(2) 9 = r 1 (H4)//i. Taking advantage of the retraction map, we can 
define the discrete Lagrangian and the discrete constraints on the Lie algebra, that 
is T d : 3(S 1 ) x 2se(2) -+ R, : 3(5 1 ) x 2se(2) -+R, a = 1,2 (with some abuse of 
notation, we employ the same notation, that is Ld and $ for the Lagrangian and 
constraints in both spaces). To consider the Lie algebra instead of the Lie group 
shall be useful since the Lie algebra is a vector space and, moreover, we stay in the 
space where the original system, i.e. L, is defined. Namely 

L d (lk, lk+1, lk+2, 6, 6+1) + ^^dilk, lk+1, lk+2, 6, 6+1) = 

,1k + lk+1 + lk+2 lk+2 - Ik lk+2 - %lk+l +lk 6 + 6+1 6+1 - 6 \ 



hL{- 



2h h 2 2 h 



Xkfoaf jk + lk+1 + lk+2 lk+2 ~ Ik lk+2 ~ ^lk+1 + Ik 6 + 6+1 6+1 ~ 6 - 
oc I 3 , 2h , h2 2 , h 
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where again we take symmetric approximations to 7^ 



'7fc+7fc+i+7fc+2 ' 



and£ fc 



Finally, as in £5^2 applying the usual calculus of variations we obtain the discrete 
equations of motion: 



= I>iL <J | € (7 fc ,7 fc+1 ,7 fc+2 ) + A*Di$2ls(7fc > 7Jfc+i > 7fc+2) 
+ D 2 L d \^(j k ^x, 7 fc , 7 fc+ i) + X k a - 1 D 2 ^2\dlk-i, Ik, 7fe+0 
+ Ai-kdl* (7fc-2, 7*-i> 7fc) + A*~ 2 I>3^d k(7fc-2, 7fc-i, 7fc) 



(20a) 



= Ad^^dr^J* (D^fa-ub) + D 2 L d | 7 (a-2,6-i)) (20b) 

- (*7/ fc r ^1^17(^)^+1) + ^2^17(^-1,^)) 

+ Ad^^dr^J* (A*- 1 ^^^) + A*r a A,^| 7 (e fc _ ai &_i)) 

- (dr^)* (A^^l^e^Cfc+i) + A^A^fe-i^)) , 



= $|ft7fc,7*+i,7fc+2,&,&+i) > fc = 0,...,iV-2; a = 1, 2. (20c) 
As before, the notation 1/^1^, $"|^ denotes that the sc(2) are frozen while, corre- 



spondigly, Ld\ 7 , $"| 7 denotes that the S 1 variables are frozen. To derive (20b) the 



properties of the right-trivialized derivative of the retraction map (and inverse), 



defined in proposition 2.1, have been used (see [TJ |26l |27]). In order to obtain 
the complete set of unknowns, that is Jo-.n, (,o-.n, A° :7V ~ 2 , we also have to take into 
account the reconstruction equation, which in this case has the form 

9h+i = 9kr(h£ k ), (21) 



where g k G SE{2). Finally, the range of validity of equations (20a) and (20b) is 
k = 2, N — 2. 

As was established in § |5.2| due to the variational procedure (70, 71) and (jn-i, Jn) 
are fixed, which leaves ^2-.n-2 as unknowns (i.e. N — 3 unknowns). By the same 



variational procedure (g , gi) and (gN-i, fl'iv) are also fixed, which by means of (21 ) 
imply that £0 and £,n-i are fixed. Nevertheless, due to the reconstruction discretiza- 
tion gk+i = gk T (h£k), is clear that fixing implies constraints in the neighboring 
points, in this case g k +\ and g k . If we allow that means constraints at the 
points «7jv+i and g^. Since we only consider time points up to T = Nh, having a 
constraint in the beyond-terminal configuration <7jv+i makes no sense. Hence, to 
ensure that the effect of the terminal constraint on £ is correctly accounted for, the 
set of algebra points must be reduced to £o:iV-i- Furthermore, since £ and (,n-i 
are also fixed, the final set of algebra unknowns reduces to £i : iV-2 0- e - 3(A — 2) 
unknowns, since dimse(2) = 3). 

On the other hand, the boundary condition g(T) (recall that we are considering 
an optimal control problem), is enforced by the relation r -1 (g^ 1 g(T)) = 0, which 
basically means that g^ — g(T). It is possible to translate this condition in terms 
of algebra elements as 

r- 1 (r(h^ 1 )- 1 ...r(h^)- 1 g l g(T)) = 0. (22) 
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We have 2(N — 1) extra unknowns when adding the Lagrange multipliers A 
(recall that, in this case a = 1, 2). Summing up, we have 

(N - 3) + 3(N - 2) + 2(N - 1) 

unknowns (corresponding to 72^-2 + £i-.n-2 + ^a N ~ 2 ) f° r 

(N - 3) + 3(iV - 3) + 3 + 2(N - 1) 



0:N- 
a 



equations (corresponding to ( |20a )+ ( |20b ) + (22) + (20c)). Consequently, our dis- 
crete variational problem (which comes from the original optimal control problem) 
has become a nonlinear root finding problem. From the set £o:iV-i we can recon- 



struct the configuration trajectory by means of the reconstruction equation (21). 



As was mentioned just after proposition 2.1, for computational reasons is useful to 



consider the retraction map r as the Cayley map for SE{2) instead of a truncation 
of the exponential map (see the Appendix for further details). 

We also would like to stress that derivation of these discrete equations have a 
pure variational formulation and as a consequence, the integrators defined in this 
way are symplectic or Poisson-momentum preserving (see [37]). By using backward 
error analysis, it is well known that these integrators have a good energy behavior 
[2]). Similar techniques have been employed in [26] for the discrete optimal 



sec 



control of an underwater vehicle on SE(3). 



5.3.2. Optimal Control of a Homogeneous Ball on a Rotating Plate. We consider the 



following well-known problem (see [HI [HI E2J I2"H]). namely the model of a homo- 
geneous ball on a rotating plate. A (homogeneous) ball of radius r > 0, mass m 
and inertia mk 2 about any axis rolls without sliding on a horizontal table which 
rotates with angular velocity Q about a vertical axis £3 through one of its points. 
Apart from the constant gravitational force, no other external forces are assumed 
to act on the sphere. Let (x, y) be denote the position of the point of contact of the 
sphere with the table. The configuration space of the sphere is Q = R 2 x 5*0(3) 
where may be parametrized Q by (x,y,g), g G £0(3), all measured with respect 
to the inertial frame. Let co = (u x ,u y ,co z ) be the angular velocity vector of the 
sphere measured also with respect to the inertial frame. The potential energy is 
constant, so we may put V = 0. 




The nonholonomic constraints are given by the non-sliding condition by 
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x + -Tr(gg T E 2 ) = -Sly, 

V - 2 Tr (99 T Ei) = Mx, 

where {Ex, E 2 , E 3 } is the standard basis of so (3). 

The matrix gg T is skew-symmetric therefore we may write 

/ -u) 3 u 2 \ 
99 T = I w 3 -ui 

\ -uj 2 uji J 

where u 2 , U3) represents the angular velocity vector of the sphere measured 
with respect to the inertial frame. Then, we may rewrite the constraints in the 
usual form: 



x — ru 2 = —Sly, 
y + ruji = Six. 

In addition, since we do not consider external forces the Lagrangian of the system 
corresponds with the kinetic energy 

X 

K(x, y, g, x, y, g) = -(mi 2 + my 2 + mk 2 {uj\ + uj\ + wf )). 

Observe that the Lagrangian is metric on Q which is bi-invariant on SO (3) as 
the ball is homogeneous. 

Now, it is clear that Q = R 2 x 50(3) is the total space of a trivial principal 
50(3)-bundle over R 2 with respect the right 50(3)— action given by (x,y,R) 
(x, y, RS) for all 5 G 50(3) and (x, y, R) e R 2 x 50(3). The action is in the right 
side since the symmetries are material symmetries. 

The bundle projection : Q — > M = R 2 is just the canonical projection on 
the first factor. Therefore, we may consider the corresponding quotient bundle 
E = TQ/S0(3) over M = R 2 . We will identify the tangent bundle to 50(3) 
with so (3) x 50(3) by using right translation. Note that throughout the previous 
exposition we have employed the left trivialization. We use the right translation in 
this example for sake of simplicity. However, we would like to point out that the 
right trivialization just implies minor chanes in the derivation of the equations of 
motion (see [21]). 

Under this identification between T(50(3)) and so (3) x 50(3), the tangent 
action of 50(3) on T(50(3)) ^ so(3) x 50(3) is the trivial action 

(so(3) x 50(3)) x 50(3) so (3) x 50(3), ((w, g),h)t-> (u, gh). (23) 

Thus, the quotient bundle TQ / 'SO (3) is isomorphic to the product manifold TR 2 x 
so(3), and the vector bundle projection is tr2 opn, where pr\ : TR 2 xso(3) — > TR 2 
and t ir2 : TR 2 — > R 2 are the canonical projections. 

A section of E = TQ/S0{3) ^ TR 2 x so (3) ->■ R 2 is a pair (X, /), where X is a 
vector field on R 2 and / : R 2 — > so (3) is a smooth map. Therefore, a global basis 
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of sections of TW x 50 (3) -»■ R 2 is 

6l = ( £' 0) ' 62 = ( |' 0) ' 

e 3 = (0,Ei), e 4 = (0,£ 2 ), e 5 = (0,£ 3 ). 

There exists a one-to-one correspondence between the space T(E = TQ/SO(3)) 
and the G-invariant vector fields on Q. 

If [•, •] is the Lie bracket on the space T(E = TQ/SO(3)), then the only non-zero 
fundamental Lie brackets are 

[e 4 , e 3 ] = e 5 , [e 5 , e 4 ] = e 3 , [e 3 , e 5 ] = e 4 . 

Moreover, it follows that the Lagrangian function L = K and the constraints 
are S'0(3)-invariant. Consequently, L induces a Lagrangian function V on E = 
TQ/SO{3) ~ TR 2 x so(3). 

We have a constrained system on i£ = TQ / SO{3) ~ TR 2 x so(3) and note that 
in this case the constraints are nonholonomic and affine in the velocities. This 
kind of systems was recently analyzed by J. Cortes et al [Ej (in particular, this 
example was carefully studied). The constraints define an affine subbundle of the 
vector bundle E ~ TR 2 x so (3) — > R 2 which is modeled over the vector subbundle 
D generated by the sections 

D = span{e 5 ; re\ + e 4 ; re 2 - e 3 } 

Moreover, the angular momentum of the ball about the axis x 3 is a conserved 
quantity since the Lagrangian is invariant under rotations about the axis x 3 and 
the infinitesimal generator for these rotations lies in the distribution r D. The con- 
servation law is written as u z = c, where c is a constant or as u z = 0. Then by the 
conservation of the angular momentum the second order constraints appear. 

After some computations the equations of motion for this constrained system 
are precisely 

x — ru)2 = —Qy, 

y + rui = Qx, (24) 
^3 = 

together with 

y r 2 _|_ 

Now, we pass to an optimization problem. Assume full control over the motion 
of the center of the ball (the shape variables). The controlled system can be written 



k 2 n 
k 2 n 

y-^w* = U2 > 



26 LEONARDO COLOMBO, FERNANDO JIMENEZ, AND DAVID MARTIN DE DIEGO 

subject to 



ux + \y = ^, (25) 

u 3 = 0. 



Next, we consider the optimal control problem for this system following the 
techniques proposed in this paper. 

Let C be the cost function in our optimization problem: 



we will want to minimize the total energy of the system. Given q(0),q(T) G R , 
g(0) G T q{0) R 2 ,q(T) G T q{T) R 2 , q = (x,y) G R 2 , w(0),w(T) G so(3), we look for an 
optimal control curve (q(t),uj(t),u(t)) on the reduced space that steers the system 
from (g(0),o;(0)) to (q(T),u(T)) minimizing 



o 



2 ( u l + U T) dt, 



subject to the constraints given by equations (25). (Recall also that R(0),R(T) G 
5*0(3), the initial and final configurations of the problem, are also fixed. Its dy- 
namics is given by the continuous reconstruction equation R(t) = R(t)u(t).) 

As in the previous example, we define the second order Lagrangian L : T^R 2 x 
2so(3) ->■ R given by 



if k 2 n \ 2 i / k 2 n 



L (x,y,x,y,x,y, u h U2,u)3, u 3 ) = - f x + ^ + fc2 y J + 2 ~ r 2 + k 2 ± ' ^ 



subject to second-order constraints $ Q : T^ 2 ^R 2 x 2so(3) — > R, a = 1,2,3. 



1 1 

& = u 1 + -y-— , (27a) 
$ 2 = uj 2 --x-^ J -, (27b) 

^3 



<T = w 3 . (27c) 



As established in proposition |5.1[ as a pure constrained variational problem, the 
optimal control problem is prescribed by solving the following system of 4-order 
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. n a 2 (lv) 2k 2 n'y k 4 n 2 x 

Ax — I hi 1 ' + 



r r r 2 + k 2 (r 2 + k 2 ) 2 

A 2 - + — 



= Ai + A2W3 - A3W2, 
= A 2 - A1W3 + A3W1, 

= A 3 + Aiw 2 - A 2 wi, 

f Qx 

= u)i + -y , 

1 

= 0J2 x , 

= u 3 . 

In addition, as mentioned before, the configurations R G SO (3) are given by the 
continuous reconstruction equation R = Roj. 

In the particular case when the angular velocity Q depends on the time (see 
[S El] ) , the equations of motion are rewritten as 

0(t) A 2 fc 2 fT(t)y ^ 2fc 2 ^(t)y , 2k 2 Q(t)y 

U — Ai 1 hi H ^— — 715 1 7, t~o r 



+ 



r r r 2 + k 2 r 2 + k 2 r 2 + k 2 ' 

k 2 fi'{t)y k 4 n 2 (t)x 2k 4 n'{t)n(t)x 

r 2 + k 2 ~ (r 2 + P) 2 (r 2 + A; 2 ) 2 

fi(t) Aj _ W(t)x 3A; 2 ft'(t)x 2k 2 n{t)x 

2 r r ^ r 2 + k 2 r 2 + k 2 r 2 + k 2 ' 

k 4 Q 2 (t)y 2k 4 n(t)n'(t) y 

(r 2 + y 2 ) 2 (r 2 + A; 2 ) 2 ' 
= Ai + A 2 w 3 - A 3 w 2 , 
= A 2 - A1W3 + A3W1, 
= A 3 + Aiw 2 - A 2 wi, 
1 n(t)x 

= ^ + -y-^^^ 

= u 2 x , 

= w 3 . 



• Discrete setting: As in the previous example, we discretize this problem 
by choosing a discrete Lagrangian and discrete constraints Employing 
equivalent arguments than in the previous example, we set L d : 3(R 2 ) x 2so(3) — > R, 
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and $^ : 3(E 2 ) x 2so(3) -»• R, a = 1, 2, 3, as 

Ld(qk, Qk+i, Qk+2, wjfc, Wfc+i) + A Q <I>2 (gfc, ?fe+i, ?fc+2, ^fc, ^k+i) = 

_ , J ,Qk + Qk+l + ?fe+2 Qk+2 ~ Ik Qk+2 — ^Qk+l + C/fc t^fc + Wfc+i — 0>fc . 

\ \k fact ( Qk + Qk+l + Qk+2 Qk+2 ~ Qk Qk+2 ~ ^Qk+1 + Qk ^k + ^k+1 ^k+l—^k-. 

We employ the same unknowns-equations counting process than in the previous 
example to find out that the number of unknowns matches the number of equa- 
tions. Therefore, our discrete variational problem (which comes from the original 
optimal control problem) has become again a nonlinear root finding problem. For 
computational reasons is useful to consider the retraction map r as the Cayley 
map for 50(3) instead of a truncation of the exponential map (see the Appendix 
for further details). 



6. Conclusions and Future work 

6.1. Conclusions. In this paper, we have designed new variational integrators for 
the optimal control of underactuated mechanical systems, showing how develop- 
ments in the theory of discrete mechanics and variational methods with constraints 
can be used to construct numerical optimal control algorithms with certain geo- 
metric desirable features. The methods are available for developing integrators 
on higher-order problems. The main idea is to use discrete variational calculus 
when the configuration space is a trivial principal bundle for systems with higher- 
order constraints and to derive the discrete Euler-Lagrange equation for discrete 
Lagrangians corresponding to a discretization of the higher-order Lagrangians. 

It is also possible to use our techniques and the numeric integrators obtained 
for other interesting problems, for instance, the theory of fc-splines on 5*0(3) [22], 
jH] . In this paper, we show two applications of our ideas to the optimal control of 
mechanical systems defined on trivial principal bundles: an underactuated vehicle 
and a (homogeneous) ball rotating on a plate. 

6.2. Future Work. A complete study of symmetry reduction, discrete Hamilton- 
ian description, preservation of geometric structure and numerical simulations will 
be developed in a future paper. This discrete approach will be also studied and 
adapted to the Lie groupoid setting (see (EH [31} E7]). Another interesting point is 
to extend our methods to underactuated constrained systems using discrete con- 
strained variational calculus (see (16] for the continuous counterpart). The case of 
optimal control problems for mechanical systems with nonholonomic constraints 
will be also studied using some of the ideas exposed through the paper (see [23] 
for more details). 



Appendix: The Cayley map 
The Cayley map cay : $j —> G is defined by 

cay(0 = (e-^)- 1 (e+^) 
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and is valid for a general class of "quadratic groups" (see [23]) that include the 
groups of interest in this paper (e.g. SO(3), SE{2) and SE{3)). Its right trivialized 
derivative and inverse are defined by 

dcay^y = (e - y (e + , 
dcay^y = (e--)y(e + |). 

The exact form of the Cayley map for SE(2) and SO(3) is given in the following 
subsections. 

The Cayley map for SE(2). The coordinates of SE(2) are (6,x,y) with matrix 
representation g £ SE(2) given by 

cos 8 — sin9 x 
sin 9 cos6 y 
1 

Using the isomorphic map : : R 3 — > se(3) given by: 

-1>l t> 2 

vi v 3 


where v = (t>i, t>2, t>2) T £ R 3 - Thus, {ei,e2,e 3 } can be used as a basis for se(3), 
where {ei,e2,e 3 } is the standard basis of R 3 . The map cay : se(3) — > SE(2) is 
given by 

/ i ( A-v\ -Av x ~2v 1 v 3 + 4f 2 
cay(v) = 4+v i \ 4t> ! 4 - f \ 2v x v 2 + 4w 3 
\ 1 

while the map drr 1 becomes the 3x3 matrix 



where 



dcay fi 1 = I 3 - ^ad v + ^ (viv 3x2 ) 



ad, 





The Cayley map for 50(3). the group of rigid body rotations is represented 
by 3 x 3 matrices with orthonormal column vectors corresponding to the axes of 
a right-handed frame attached to the body. On the other hand, the algebra so (3) 
is the set of 3 x 3 antisymmetric matrices. A so (3) basis can be constructed as 
{ei,e 2 ,e 3 }, £ so(3), where {ei,e 2 ,e 3 } is the standard basis for R 3 . Elements 
£ £ so (3) can be identified with the vector u £ R 3 through £ = u a e a , or £ = Cj. 
Under such identification the Lie bracket coincides with the standard cross product, 
i.e., adi p = u x p, for some p £ R 3 . Using this identification and recalling the hat 
isomorphism : defined above, we have 

cay(c2,) = I 3 + 4+ * U + y) , (28) 
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where J3 is the 3x3 identity. The linear maps dr^ and dr^ are expressed as the 
3x3 matrices 

dcay„= 4+ j* (2I3+&), dcay- 1 = / 3 -| + ^. (29) 
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